home *** CD-ROM | disk | FTP | other *** search
/ BCI NET 2 / BCI NET 2.iso / archives / programming / source / graphicgems4.lha / GemsIV / vec_mat / ray / solver.c < prev    next >
Encoding:
C/C++ Source or Header  |  1995-02-06  |  5.1 KB  |  267 lines
  1. #include "solver.h"
  2.  
  3. /********************************************************
  4. *                            *
  5. * This function determines if a double is small enough    *
  6. * to be zero. The purpose of the subroutine is to try    *
  7. * to overcome precision problems in math routines.    *
  8. *                            *
  9. ********************************************************/
  10.  
  11. static int isZero(double x)
  12. {
  13. return x > -EQN_EPS && x < EQN_EPS;
  14. }
  15.  
  16.  
  17. int solveLinear(double c[2], double s[1])
  18. {
  19. if (isZero(c[1]))
  20.     return 0;
  21. s[0] = - c[0] / c[1];
  22. return 1;
  23. }
  24.  
  25.  
  26.  
  27. /********************************************************
  28. *                            *
  29. * This function determines the roots of a quadric    *
  30. * equation.                        *
  31. * It takes as parameters a pointer to the three        *
  32. * coefficient of the quadric equation (the c[2] is the    *
  33. * coefficient of x2 and so on) and a pointer to the    *
  34. * two element array in which the roots are to be    *
  35. * placed.                        *
  36. * It outputs the number of roots found.            *
  37. *                            *
  38. ********************************************************/
  39.  
  40. int solveQuadric(double c[3], double s[2])
  41. {
  42. double p, q, D;
  43.  
  44.  
  45. // make sure we have a d2 equation
  46.  
  47. if (isZero(c[2]))
  48.     return solveLinear(c, s);
  49.  
  50.  
  51. // normal for: x^2 + px + q
  52. p = c[1] / (2.0 * c[2]);
  53. q = c[0] / c[2];
  54. D = p * p - q;
  55.  
  56. if (isZero(D))
  57.     {
  58.     // one double root
  59.     s[0] = s[1] = -p;
  60.     return 1;
  61.     }
  62.  
  63. if (D < 0.0)
  64.     // no real root
  65.     return 0;
  66.  
  67. else
  68.     {
  69.     // two real roots
  70.     double sqrt_D = sqrt(D);
  71.     s[0] = sqrt_D - p;
  72.     s[1] = -sqrt_D - p;
  73.     return 2;
  74.     }
  75. }
  76.  
  77.  
  78.  
  79. /********************************************************
  80. *                            *
  81. * This function determines the roots of a cubic        *
  82. * equation.                        *
  83. * It takes as parameters a pointer to the four        *
  84. * coefficient of the cubic equation (the c[3] is the    *
  85. * coefficient of x3 and so on) and a pointer to the    *
  86. * three element array in which the roots are to be    *
  87. * placed.                        *
  88. * It outputs the number of roots found            *
  89. *                            *
  90. ********************************************************/
  91.  
  92. int solveCubic(double c[4], double s[3])
  93. {
  94. int    i, num;
  95. double    sub,
  96.     A, B, C,
  97.     sq_A, p, q,
  98.     cb_p, D;
  99.  
  100. // normalize the equation:x ^ 3 + Ax ^ 2 + Bx  + C = 0
  101. A = c[2] / c[3];
  102. B = c[1] / c[3];
  103. C = c[0] / c[3];
  104.  
  105. // substitute x = y - A / 3 to eliminate the quadric term: x^3 + px + q = 0
  106.  
  107. sq_A = A * A;
  108. p = 1.0/3.0 * (-1.0/3.0 * sq_A + B);
  109. q = 1.0/2.0 * (2.0/27.0 * A *sq_A - 1.0/3.0 * A * B + C);
  110.  
  111. // use Cardano's formula
  112.  
  113. cb_p = p * p * p;
  114. D = q * q + cb_p;
  115.  
  116. if (isZero(D))
  117.     {
  118.     if (isZero(q))
  119.     {
  120.     // one triple solution
  121.     s[0] = 0.0;
  122.     num = 1;
  123.     }
  124.     else
  125.     {
  126.     // one single and one double solution
  127.     double u = cbrt(-q);
  128.     s[0] = 2.0 * u;
  129.     s[1] = - u;
  130.     num = 2;
  131.     }
  132.     }
  133. else
  134.     if (D < 0.0)
  135.     {
  136.     // casus irreductibilis: three real solutions
  137.     double phi = 1.0/3.0 * acos(-q / sqrt(-cb_p));
  138.     double t = 2.0 * sqrt(-p);
  139.     s[0] = t * cos(phi);
  140.     s[1] = -t * cos(phi + M_PI / 3.0);
  141.     s[2] = -t * cos(phi - M_PI / 3.0);
  142.     num = 3;
  143.     }
  144.     else
  145.     {
  146.     // one real solution
  147.     double sqrt_D = sqrt(D);
  148.     double u = cbrt(sqrt_D + fabs(q));
  149.     if (q > 0.0)
  150.         s[0] = - u + p / u ;
  151.     else
  152.         s[0] = u - p / u;
  153.     num = 1;
  154.     }
  155.  
  156. // resubstitute
  157. sub = 1.0 / 3.0 * A;
  158. for (i = 0; i < num; i++)
  159.     s[i] -= sub;
  160. return num;
  161. }
  162.  
  163.  
  164.  
  165.  
  166. /********************************************************
  167. *                            *
  168. * This function determines the roots of a quartic    *
  169. * equation.                        *
  170. * It takes as parameters a pointer to the five        *
  171. * coefficient of the quartic equation (the c[4] is the    *
  172. * coefficient of x4 and so on) and a pointer to the    *
  173. * four element array in which the roots are to be    *
  174. * placed. It outputs the number of roots found.        *
  175. *                            *
  176. ********************************************************/
  177.  
  178. int
  179. solveQuartic(double c[5], double s[4])
  180. {
  181. double        coeffs[4],
  182.         z, u, v, sub,
  183.         A, B, C, D,
  184.         sq_A, p, q, r;
  185. int        i, num;
  186.  
  187.  
  188. // normalize the equation:x ^ 4 + Ax ^ 3 + Bx ^ 2 + Cx + D = 0
  189.  
  190. A = c[3] / c[4];
  191. B = c[2] / c[4];
  192. C = c[1] / c[4];
  193. D = c[0] / c[4];
  194.  
  195. // subsitute x = y - A / 4 to eliminate the cubic term: x^4 + px^2 + qx + r = 0
  196.  
  197. sq_A = A * A;
  198. p = -3.0 / 8.0 * sq_A + B;
  199. q = 1.0 / 8.0 * sq_A * A - 1.0 / 2.0 * A * B + C;
  200. r = -3.0 / 256.0 * sq_A * sq_A + 1.0 / 16.0 * sq_A * B - 1.0 / 4.0 * A * C + D;
  201.  
  202. if (isZero(r))
  203.     {
  204.     // no absolute term:y(y ^ 3 + py + q) = 0
  205.     coeffs[0] = q;
  206.     coeffs[1] = p;
  207.     coeffs[2] = 0.0;
  208.     coeffs[3] = 1.0;
  209.  
  210.     num = solveCubic(coeffs, s);
  211.     s[num++] = 0;
  212.     }
  213. else
  214.     {
  215.     // solve the resolvent cubic...
  216.     coeffs[0] = 1.0 / 2.0 * r * p - 1.0 / 8.0 * q * q;
  217.     coeffs[1] = -r;
  218.     coeffs[2] = -1.0 / 2.0 * p;
  219.     coeffs[3] = 1.0;
  220.     (void) solveCubic(coeffs, s);
  221.  
  222.     // ...and take the one real solution...
  223.     z = s[0];
  224.  
  225.     // ...to build two quadratic equations
  226.     u = z * z - r;
  227.     v = 2.0 * z - p;
  228.  
  229.     if (isZero(u))
  230.     u = 0.0;
  231.     else if (u > 0.0)
  232.     u = sqrt(u);
  233.     else
  234.     return 0;
  235.  
  236.     if (isZero(v))
  237.     v = 0;
  238.     else if (v > 0.0)
  239.     v = sqrt(v);
  240.     else
  241.     return 0;
  242.  
  243.     coeffs[0] = z - u;
  244.     coeffs[1] = q < 0 ? -v : v;
  245.     coeffs[2] = 1.0;
  246.  
  247.     num = solveQuadric(coeffs, s);
  248.  
  249.     coeffs[0] = z + u;
  250.     coeffs[1] = q < 0 ? v : -v;
  251.     coeffs[2] = 1.0;
  252.  
  253.     num += solveQuadric(coeffs, s + num);
  254.     }
  255.  
  256. // resubstitute
  257. sub = 1.0 / 4 * A;
  258. for (i = 0; i < num; i++)
  259.     s[i] -= sub;
  260.  
  261. return num;
  262.  
  263. }
  264.  
  265.  
  266.  
  267.